#pragma config(Hubs,  S1, HTMotor,  HTServo,  none,     none)
#pragma config(Sensor, S2,     HTSMUX,                sensorLowSpeed)
#pragma config(Motor,  mtr_S1_C1_1,     motorD,        tmotorNormal, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     motorE,        tmotorNormal, openLoop, reversed, encoder)
#pragma config(Servo,  srvo_S1_C2_1,    ,                     tServoStandard)
#pragma config(Servo,  srvo_S1_C2_2,    ,                     tServoStandard)
#pragma config(Servo,  srvo_S1_C2_3,    ,                     tServoStandard)
#pragma config(Servo,  srvo_S1_C2_4,    ,                     tServoStandard)
#pragma config(Servo,  srvo_S1_C2_5,    ,                     tServoStandard)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "autonomousAtTheBridge.h"
#include "JoystickDriver.c"

#define START_FROM_LEFT false
#define START_FROM_RIGHT true

void initializeRobot()
{
  wait1Msec(1000);
  ClearTimer(T1);
  ClearTimer(T2);
  servoChangeRate[servo1] = 1;
  servoChangeRate[servo6] = 10;
  servoChangeRate[servo3] = 10;
  servo[servo1] = 250;// raise the robots arms to starting position
  servo[servo6] = 160;
  servo[servo3] = 95;
  servo[servo5] = 10;
  servo[servo4] = 245;
  motor[motorA] = 0;
  wait1Msec(1000);
  return;
}

void autonomousToBridge(bool startSide)
{
  initializeRobot();

  waitForStart();
  // drive forward towards bridge
  servo[servo6] = 80;// lower scoop
  servo[servo3] = 175;
  servo[servo5] = 50;
  servo[servo4] = 205;
  motor[motorE] = 0;// go forward to the bridge
  motor[motorD] = 0;
  nMotorEncoder[motorE] = 0;
  nMotorEncoder[motorD] = 0;
  nMotorEncoderTarget[motorE] = 4500;
  nMotorEncoderTarget[motorD] = 4500;
  motor[motorD] = 80;
  motor[motorE] = 80;
  ClearTimer(T2);
  while (nMotorRunState[motorE] != runStateIdle && nMotorRunState[motorD] != runStateIdle && time1[T2] < 3000);
// turn to face bridge (checks what program is running, and turns the appropriate direction
  if (startSide == START_FROM_RIGHT)
  {
    motor[motorE] = 0;
    motor[motorD] = 0;
    nMotorEncoder[motorE] = 0;
    nMotorEncoderTarget[motorE] = 1500;
    motor[motorE] = 80;
    ClearTimer(T2);
    while (nMotorRunState[motorE] != runStateIdle && time1[T2] < 2000);
  }
  else
  {
    motor[motorE] = 0;
    motor[motorD] = 0;
    nMotorEncoder[motorD] = 0;
    nMotorEncoderTarget[motorD] = 1500;
    motor[motorD] = 80;
    ClearTimer(T2);
    while (nMotorRunState[motorD] != runStateIdle && time1[T2] < 2000);
  }
  // drive onto bridge
  motor[motorE] = 0;
  motor[motorD] = 0;
  nMotorEncoder[motorE] = 0;
  nMotorEncoder[motorD] = 0;
  nMotorEncoderTarget[motorE] = 4000;
  nMotorEncoderTarget[motorD] = 4000;
  motor[motorD] = 80;
  motor[motorE] = 80;
  ClearTimer(T2);
  while (nMotorRunState[motorE] != runStateIdle && nMotorRunState[motorD] != runStateIdle && time1[T2] < 2000);

  motor[motorD] = 0;
  motor[motorE] = 0;
  servo[servo5] = 180;// raise arm and open bottom of scoop so SONAR can see the other robot
  servo[servo4] = 74;
  servo[servo6] = 160;
  servo[servo3] = 95;
  autonomousAtTheBridge();
}
